#! /bin/bash

# 清理旧日志，只保留最近的3份
LOG_BASE_DIR="$HOME/logs"
if [ -d "$LOG_BASE_DIR" ]; then
    echo "清理旧日志文件夹，只保留最近的3份..."
    # 获取所有yacht_开头的文件夹，按修改时间排序，保留最新的3个，删除其余的
    ls -dt "$LOG_BASE_DIR"/yacht_* 2>/dev/null | tail -n +4 | xargs -r rm -rf
    if [ $? -eq 0 ]; then
        echo "旧日志清理完成"
    fi
fi

# 创建日志目录
LOG_DIR="$HOME/logs/yacht_$(date +%Y%m%d_%H%M%S)"
mkdir -p "$LOG_DIR"

echo "日志将保存到: $LOG_DIR"

# 声明关联数组
declare -A NODE_PIDS
declare -A LOG_FILES

# 启动 Livox 驱动
source /home/firefly/ws_livox/devel/setup.bash
# stdbuf -o0 -e0 roslaunch livox_ros_driver2 wangjian.launch > "$LOG_DIR/livox_driver.log" 2>&1 &
stdbuf -o0 -e0 roslaunch livox_ros_driver2 msg_MID360.launch > "$LOG_DIR/livox_driver.log" 2>&1 &
NODE_PIDS["livox"]=$!
LOG_FILES["livox"]="$LOG_DIR/livox_driver.log"

# 启动串口节点 - 禁用缓冲
source /home/firefly/ws_pixhawk/devel/setup.bash
stdbuf -o0 -e0 rosrun pegasus serialport_node _port:=/dev/ttyUSB0 _baud_rate:=115200 > "$LOG_DIR/serialport.log" 2>&1 &
NODE_PIDS["serialport"]=$!
LOG_FILES["serialport"]="$LOG_DIR/serialport.log"

# 启动 Navigation Geometry Transform
sleep 2
# 启动 Point-LIO
source /home/firefly/ws_livox/devel/setup.bash
stdbuf -o0 -e0 roslaunch point_lio mapping_mid360.launch > "$LOG_DIR/point_lio.log" 2>&1 &
NODE_PIDS["point_lio"]=$!
LOG_FILES["point_lio"]="$LOG_DIR/point_lio.log"

# 启动 Nav Geometry Transform
source /home/firefly/ws_pixhawk/devel/setup.bash
stdbuf -o0 -e0 /home/firefly/ws_pixhawk/src/pegasus/scripts/nav_geo_trans.py > "$LOG_DIR/nav_geo_trans.log" 2>&1 &
NODE_PIDS["nav_geo_trans"]=$!
LOG_FILES["nav_geo_trans"]="$LOG_DIR/nav_geo_trans.log"

# 启动 PX4
stdbuf -o0 -e0 roslaunch mavros px4.launch > "$LOG_DIR/mavros.log" 2>&1 &
NODE_PIDS["mavros"]=$!
LOG_FILES["mavros"]="$LOG_DIR/mavros.log"

# 启动 Drone Control
source /home/firefly/ws_pixhawk/devel/setup.bash
stdbuf -o0 -e0 rosrun pegasus drone_control > "$LOG_DIR/drone_control.log" 2>&1 &
NODE_PIDS["drone_control"]=$!
LOG_FILES["drone_control"]="$LOG_DIR/drone_control.log"

# 创建主日志文件记录 PIDs
echo "启动时间: $(date)" > "$LOG_DIR/startup.log"
for node in "${!NODE_PIDS[@]}"; do
    echo "$node PID: ${NODE_PIDS[$node]}" >> "$LOG_DIR/startup.log"
done

# 等待用户中断
echo "所有节点已启动，日志保存在 $LOG_DIR"
echo "按 Ctrl+C 停止所有节点"
echo ""
echo "可用的日志查看命令："
echo "查看所有节点: tail -f ${LOG_FILES[*]}"
for node in "${!LOG_FILES[@]}"; do
    echo "查看 $node: tail -f ${LOG_FILES[$node]}"
done
echo ""
echo "按 Enter 键开始默认监控，或打开新终端使用上述命令查看特定日志..."

# 捕获中断信号 - 修复这里的变量引用
trap 'echo "停止所有节点..."; for pid in "${NODE_PIDS[@]}"; do kill $pid 2>/dev/null; done; pkill -f "tail -f"; exit 0' INT

# 等待用户按Enter开始监控
read -r

# 启动实时日志监控
echo "开始实时监控日志输出..."
echo "=================================="

# 使用 tail -f 实时监控所有日志文件
tail -f "${LOG_FILES[@]}" &
TAIL_PID=$!

# 保持脚本运行
wait